"""
About
"""
import os, sys, time, math, fileinput
try:
	import serial
except ImportError, e:
	print "\nYou must first download and install PySerial"
	print "\'user$ pip install pyserial\'\n"
	sys.exit()


#pc_file_header = "# .PCD v.7 - Point Cloud Data file format\nVERSION\
# .7\nFIELDS x y z\nSIZE 4 4 4\nTYPE F F F\nCOUNT 1 1 1\nVIEWPOINT 0 0 0 1 0 0 0\nDATA ascii\n"

pc_file_header = "ply\n\
format ascii 1.0\n\
element vertex\n\
property float32 x\n\
property float32 y\n\
property float32 z\n\
element face 0\n\
property list uchar int vertex_indices\n\
end_header\n"

ser = serial.Serial(baudrate=9600, timeout=3)

AZLOWBOUND = 0
AZHIGHBOUND = 360
ALTLOWBOUND = 0
ALTHIGHBOUND = 90


def connect():
	# A function to possibly initiate serial, and then ping the microcontroller
	print "Starting program..."
	try:
		# POSIX-based <------ Add Windows support?
		for file in os.listdir("/dev"):
		    if "tty.usb" in file.lower():
				usbpath = os.path.join("/dev", file)
				print "USB connection found at: ", usbpath

		if usbpath == "":
			print "Could not find USB connection. Exiting..."
			sys.exit()
		else:
			ser.setPort(port=usbpath)
			ser.open()

			time.sleep(0.1)

			# Check connection
			ser.write("p")
			reply = ser.readline()
			if reply[0] == "p":
				print "Connection Established"
			else:
				print "Cannot establish connection. Exiting..."
				sys.exit()
	
	except Exception, e:
		raise


def inputsAndInit():
	# Welcome and Instructions
	# <-------- Fill This In
	print "Welcome and instructions"


	# Accept scanning route from user  
	azimuthLow = -1
	azimuthHigh = 361
	altLow = -1
	altHigh = 91

	#   <------- Add Confirmation / Restarting? 
	filename = raw_input("Enter Point Cloud File Name (no extension): ")
	filename = filename + ".ply"
	print "Points will be stored in:", os.path.join(os.getcwd(), filename)
	
	while not (AZLOWBOUND <= azimuthLow < azimuthHigh):
		azimuthLow = int(raw_input("Enter Low Azimuth Bound (0 <= input <= high <= 360): "))
	while not (azimuthLow <= azimuthHigh <= AZHIGHBOUND):
		azimuthHigh = int(raw_input("Enter High Azimuth Bound (0 <= low <= input <= 360): "))
	while not (ALTLOWBOUND <= altLow < altHigh):
		altLow = int(raw_input("Enter Low Altitude Bound (0 <= input <= high <= 90): "))
	while not (altLow <= altHigh <= ALTHIGHBOUND):
		altHigh = int(raw_input("Enter High Altitude Bound (0 <= low <= input <= 90): "))

	# Create point cloud file
	pc_file = open(filename, 'w')
	pc_file.write(pc_file_header)

	# Send user inputted scan to embedded program
	ser.write('s %i %i %i %i\n' % (azimuthLow, azimuthHigh, altLow, altHigh))

	return pc_file


def handleCoord(coord, pc_file):
	# Parse coord into R, theta, phi
	R = float(coord[0])
	theta = float(coord[1])
	phi = float(coord[2])

	x,y,z = sphereToRect(R, theta, phi)        # Transform coordinates
	pc_file.write('%5.3f %5.3f %5.3f\n' % (x, y, z)) # Format string and write to file



def sphereToRect(R, theta, phi):
	x = R * math.sin(theta) * math.cos(phi)
	y = R * math.sin(theta) * math.sin(phi)
	z = R * math.cos(theta)

	return (x,y,z)



def receive(pc_file):
	# While loop, parse and switch statement
	done = False
	num_coords = 0

	while not done:
		# Wait for packet
		packet = ser.readline()

		# Parse packet
		args = packet.split()

		# Process packet (python does not have switch statement)
		if args[0][0] == 'c':
			handleCoord(args[1:], pc_file)
			num_coords = num_coords + 1
			print "Received coordinate: ", args[1:]

		#elif packet[0] == 'e':

		elif args[0][0] == 's':
			print "Initiating Scan..."

		elif packet[0] == 'x':
			done = True
			print "...Scan Complete"

		else:
			print "Could not process incoming packet\n"
	return num_coords



def close(pc_file, num_coords):
	# Close PC File and update Header
	pc_file.close()

	searchExp = "element vertex"
	replaceExp = "element vertex " + str(num_coords)

	for line in fileinput.input(pc_file.name, inplace=1):
	    if searchExp in line:
	    	line = line.replace(searchExp, replaceExp)
	    sys.stdout.write(line)

	# Notify and print summary to user
	# <----------  Point Cloud summary routine
	print "Point cloud file", pc_file.name, "may be found in:\n", os.getcwd()

	# Close serial
	ser.close()

	# Safe exit
	sys.exit()


def main():

	# Wait for embedded program, update user
	connect()

	# Input from users
	pc_file = inputsAndInit()

	# Receive x,y,z coordinates back from embedded program and store in point cloud
	num_coords = receive(pc_file)

	# Complete mission and exit
	close(pc_file, num_coords)



if __name__ == '__main__':
    main()
